#include"ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float32MultiArray.h"
#include "geometry_msgs/Pose.h"
#define step_length  0.05//步长，越小分点越细

float cur_pose_x = 0.0;
float cur_pose_y = 0.0;
int step = 1;

//回调函数
void locationCallback(const geometry_msgs::Pose::ConstPtr &msg1)
{
   cur_pose_x = msg1->position.x;
   cur_pose_y = msg1->position.y;
}
int main(int argc, char *argv[])
{
   setlocale(LC_ALL,"");
   ros::init(argc,argv,"path_planning_node");
   ros::NodeHandle nh;
   ros::Subscriber sub = nh.subscribe("/location",10,locationCallback);//订阅当前位置
   ros::Publisher pub1= nh.advertise<std_msgs::Float32MultiArray>("/mpc_target_point",10);//发布终点坐标
   ros::Publisher pub2= nh.advertise<std_msgs::Float32MultiArray>("/mpc_route",10);//发布路径点

   std_msgs::Float32MultiArray mpc_target_point;
   std_msgs::Float32MultiArray route;
   mpc_target_point.data.push_back(7.5);
   mpc_target_point.data.push_back(5.5);
   while(ros::ok){
      ros::spinOnce();
      
      route.data.clear();

      float dX = mpc_target_point.data[0] - cur_pose_x;
      float dY = mpc_target_point.data[1] - cur_pose_y;
      step = std::ceil(std::max(std::abs(dX),std::abs(dY)) / step_length)
      float dx = dX/step;
      float dy = dY/step;
      for(int i=0;i<=step;i++)
      {
          route.data.push_back(cur_pose_x + i*dx);
          route.data.push_back(cur_pose_y + i*dy);
      }
      pub1.publish(mpc_target_point);//发布终点
      pub2.publish(route);           //发布路径点
   }
   return 0;
}
